#include "ai_move.h"



//Set the Maximum Lineal Velocity and Angular Velocity
//negociated at the begin of the connection
void ai_set_MaxLin_Vel(int vel)
{
	MAX_LIN_VEL = vel;
}

void ai_set_MaxAng_Vel(int vel)
{
	MAX_ANG_VEL = vel;
}

//********************** Set Movement Priority Order **********************//

// It returns the position of the domination robot
// If we own the ball the domination robot is the robot that have it.
// If we don't the domination robot is that is more approach of it.

int ai_give_domination_robot(stateGame *state)
{
  int i;
  int goalx,goaly;
  int dom_robot=0;
  //int pos_dr_x; // row position of the domination robot
  //int pos_dr_y; // col position of the domination robot
  float distance;
  float min_distance = MAX_INT;
  
	goalx = state->ball.row;
	goaly = state->ball.col;	

	for (i=0;i<5; i++)
	{
    	distance = ai_euclidean_distance(goalx,goaly, state->ours[i].row,state->ours[i].col);
		//printf("distance robot %i: %f\n",i,distance);
	 	if (distance < min_distance) 
			{dom_robot=i;}
	}

  return(dom_robot);

}

// Set the team that own the ball
int set_teamOwn_ball(stateGame *state)
{
  int distance_ours;
  int distance_theirs;


	set_domination_robot_order(state);
	set_domination_robot_theirs(state);

	distance_ours = state->own_distances[0];
	distance_theirs = state->theirs_distances[0];

	if ((distance_ours - distance_theirs ) < 2 * PROXIMITY_AREA)
		{	// Tots dos propers a la pilota anem cap a ella
			return(TRUE);
		}
	else if (distance_ours < distance_theirs){return(TRUE);}
		 else {return(FALSE);}
}
// Busca quin es el porter dins dels nostres jugadors
// i quin es el nostre camp; (-1 esquerra, 1 dreta)
void set_initial_values(stateGame *state)
{
  int i;
  int robot_col;
  int goalkeeper_col;
  int sum_col;
  int pos_keeper; //position at vector that own's the goalkeeper


	sum_col = 0;
	goalkeeper_col = 900;
	for (i=0;i<5;i++)
		{
		robot_col = state->ours[i].col;
		sum_col = sum_col + robot_col;
		if ( goalkeeper_col < abs(robot_col)) { pos_keeper = i; goalkeeper_col = abs(robot_col);}
		}
	if (sum_col < 0) state->field = -1;
	else state->field = 1;
	
	state->keeperPos = pos_keeper;
	//printf("La posicio que ocupa el goalkeeper es %i\n",state->keeperPos);
	//printf("La posicio dintre del camp es %i\n",state->field);
}


// Set at global table: domRobotListTheirs
// Coloca els seus robots en ordre segons la seva proximitat a la pilota
void set_domination_robot_theirs(stateGame *state)
{
  int i;
  int j;
  int dom_robot;
  int goalx,goaly;
  int robotx, roboty;
  float distance;
  float all_distances[5];
  float min_distance = MAX_FLOAT;


	//printf("\nOrdenacio de robots per posicions properes\n");  

	goalx= state->ball.row;
	goaly= state->ball.col;


	for (i=0;i<5; i++)
	{
		robotx =state->theirs[i].row; 
		roboty =state->theirs[i].col;

	  	all_distances[i] = ai_euclidean_distance(goalx,goaly,robotx,roboty);
		
		// Set the identifier of the zone where the robot are
		state->theirs_zones[i] = get_robot_zone(robotx, roboty);

		// Set the distances between ball and own robots
		state->theirs_distances[i] = all_distances[i];
		//printf("distance robot %i: %f\n",i,all_distances[i]);
	 	if (all_distances[i] < min_distance) 
			{min_distance = all_distances[i];
			 state->domRobotListTheirs[0]=i;
			 dom_robot=i;
			}
	}
	
	// Set dominaticon Robot order
	// First dom_robot
	// If dom_robot isn't the goalkeeper The Second dom_robot is goalkeeper
	all_distances[dom_robot]= MAX_FLOAT;
	if (dom_robot != state->keeperPos)
	{ 
		
		state->domRobotListTheirs[1] = state->keeperPos;
		// Set goalkeeper as sorted
		all_distances[0]= MAX_FLOAT;
		// Set the other movement priority positions
		
		j=2;
		while(j<5)
			{
				dom_robot=1;
				min_distance = MAX_FLOAT;	
				for (i=1;i<5;i++)
					{if (all_distances[i]<all_distances[dom_robot]) {dom_robot=i;} }

				state->domRobotListTheirs[j]= dom_robot;
				all_distances[dom_robot]= MAX_FLOAT;
				j++;
			}
			
	}
	else
	{
		// Set the other movement priority positions
		j=1;
		while(j<5)
			{
				dom_robot=1;
				min_distance = MAX_FLOAT;	
				for (i=1;i<5;i++)
					{if (all_distances[i]<all_distances[dom_robot]) {dom_robot=i;} }

				state->domRobotListTheirs[j]= dom_robot;
				all_distances[dom_robot]= MAX_FLOAT;
				j++;
			}
			
	}
}
// Set at global table: domRobotList
//Coloca els nostres robots en ordre en funcio de la proximitat a la pilota
void set_domination_robot_order(stateGame *state)
{

  int i;
  int j;
  int dom_robot;
  int goalx,goaly;
  int robotx, roboty;
  float distance;
  float all_distances[5];
  float min_distance = MAX_FLOAT;


	//printf("\nOrdenacio de robots per posicions properes\n");  

	goalx= state->ball.row;
	goaly= state->ball.col;


	for (i=0;i<5; i++)
	{
		robotx =state->ours[i].row; 
		roboty =state->ours[i].col;

	  	all_distances[i] = ai_euclidean_distance(goalx,goaly,robotx,roboty);
		
		// Set the identifier of the zone where the robot are
		state->own_zones[i] = get_robot_zone(robotx, roboty);

		// Set the distances between ball and own robots
		state->own_distances[i] = all_distances[i];
		//printf("distance robot %i: %f\n",i,all_distances[i]);
	 	if (all_distances[i] < min_distance) 
			{min_distance = all_distances[i];
			 state->domRobotList[0]=i;
			 dom_robot=i;
			}
	}
	
	// Set dominaticon Robot order
	// First dom_robot
	// If dom_robot isn't the goalkeeper The Second dom_robot is goalkeeper
	all_distances[dom_robot]= MAX_FLOAT;
	if (dom_robot != state->keeperPos)
	{ 
		
		state->domRobotList[1] = state->keeperPos;
		// Set goalkeeper as sorted
		all_distances[0]= MAX_FLOAT;
		// Set the other movement priority positions
		
		j=2;
		while(j<5)
			{
				dom_robot=1;
				min_distance = MAX_FLOAT;	
				for (i=1;i<5;i++)
					{if (all_distances[i]<all_distances[dom_robot]) {dom_robot=i;} }

				state->domRobotList[j]= dom_robot;
				all_distances[dom_robot]= MAX_FLOAT;
				j++;
			}
			
	}
	else
	{
		// Set the other movement priority positions
		j=1;
		while(j<5)
			{
				dom_robot=1;
				min_distance = MAX_FLOAT;	
				for (i=1;i<5;i++)
					{if (all_distances[i]<all_distances[dom_robot]) {dom_robot=i;} }

				state->domRobotList[j]= dom_robot;
				all_distances[dom_robot]= MAX_FLOAT;
				j++;
			}
			
	}
}

/*************************** Field Subdivisions **************************/
// Get the identifier of the zone where the robot are.{1,2,3,4,5}
int get_robot_zone(int row, int col)
{
  int zone;
  int abs_col= abs(col); 
	if ( abs_col < Region_Col_B)
		{
		 if (row <= HALF_FIELD_HEIGTH) { zone = 5;} 
		 else {zone = 4;}
		}
	else
		{
		if (abs_col < Region_Col_A)	{zone = 3;}
		else
			{ if (abs_col < Region_Goal_Left) {zone = 2;}
			  else {zone = 1;}
			}			
		}

	if (row < 0) return ( zone *(-1));
	else return (zone);
	return(zone);
}

//Subdivideix el camp en 4 parts per tal d'identificar l'accio que 
//ha de realitzar el robot
int get_robot_region(stateGame *state,int row, int col)
{
  int zone;
  int abs_col= abs(col); 

	if ( abs_col >= Region_Col_B)
	{
		if ((col >0) && (state->field == 1)) return(2);
		else return(1);
	}
	else
	{
		if (row <= HALF_FIELD_HEIGTH) {return (4);}
		else return (3);
	}
}

// Set our own field, it depends of the round that we are playing
int set_own_field(stateGame *state)
{
 printf("El round del partit es %i\n",state->round);
 if (state->round == 1){ state->field = 1;}
 else { state->field = -1;}
}

//****************************** Distances ******************************************** 
// Give the euclidean distance of two points in a 2D system.
float ai_euclidean_distance(int row1, int col1, int row2, int col2)
{
  float distance;

	distance = ((row1 - row2) * (row1 - row2)) + ((col1- col2) * (col1- col2));
	//printf("The distance is (%i, %i) to  (%i, %i) ==> %f\n",row1, col1, row2,col2,distance);
	distance = sqrt(distance);

	return(distance);
}


// Give the manhattan distance of two points in a 2D system
float ai_manhattan_distance(int row1, int col1, int row2, int col2)
{
  float distance;

	//distance = abs((double)(row1 - row2)) - abs((double)(col1- col2));
	distance=(float)abs((double)row1-row2)+(float)abs((double)col1-col2);
	//printf("sqrt(%f) = %d",(float)abs((double)row1-row2)+(float)abs((double)col1-col2));
	
	distance = sqrt((double)distance);

	return(distance);
}

/************************* Basic Robot Movements *************************/

//Indicates if there are obstacles in trajectory between ball (row,col) and robot
//Check: distance SHOT_CLEAN_AREA source
//		 distance SHOT_CLEAN_AREA destination
//		 distance SHOT_CLEAN_AREA half path between source and destination
// return 1: yes (TRUE)  -- 0:false (FALSE);
// return parameter dir : -1 left, 1 right , 0 push 
int ai_obstacles_in_trajec(stateGame *state,int row,int col, int dst, int *dir)
{

  int i;
  int pasos; // amb la velocitat maxima que pot xutar un jugador
			 // la velocitat maxima sera ::
			 // pasos = distancia / velocitat
  int direction;
  float distance;
  int valida;

  float path_row;
  float path_col;

    valida = TRUE;
		i=0;
		while ((i<5) && (valida))
		{
			//Esta lluny el contrincant del emisor
			distance = (row,col,state->theirs[i].row,state->theirs[i].col);
			if (distance < SHOOT_CLEAN_AREA) valida = FALSE;
			
			//Esta lluny el contrincant del receptor
			distance = (state->ours[i].row,state->ours[i].col,state->theirs[i].row,state->theirs[i].col);
			if (distance < SHOOT_CLEAN_AREA) valida = FALSE;

			// Comprobar si esta prou lluny del punt mig

			path_row = (float)(row - state->ours[i].row ) /2.0;

			path_col = (float)(col - state->ours[i].col ) /2.0;

			if (row <= state->ours[i].row) //increment positiu row
				{ 
				if (col <= state->ours[i].col) //increment positiu  col
					{
					distance = (row + path_row,col + path_col,state->theirs[i].row,state->theirs[i].col);
					if (distance < SHOOT_CLEAN_AREA) { *dir = (int)-1;valida = FALSE;}
					}
				else	//incremtn negatiu col
					{
					distance = (row + path_row,col - path_col,state->theirs[i].row,state->theirs[i].col);
					if (distance < SHOOT_CLEAN_AREA) { *dir =-1 ;valida = FALSE;}
					}
				}
			else //increment negatiu row
				{
				if (col <= state->ours[i].col) //increment positiu  col
					{
					distance = (row - path_row,col + path_col,state->theirs[i].row,state->theirs[i].col);
					if (distance < SHOOT_CLEAN_AREA) valida = FALSE;
					}
				else	//incremtn negatiu col
					{
					distance = (row - path_row,col - path_col,state->theirs[i].row,state->theirs[i].col);
					if (distance < SHOOT_CLEAN_AREA) valida = FALSE;
					}
				}
			
		i++;
		}
 return(valida);
}

void ai_set_shoot(stateGame *state, movements *mov,int index)
{
 int pos_x;
 int pos_y;
 int valida; // indica si el llençament es factible
 int i; //indica index del propi robot
 int dir;
 int ball_x;
 int ball_y;
 int orientation;

 pos_x = state->ours[index].row;
 pos_y = state->ours[index].col;
 
 ball_x = state->ball.row;
 ball_y = state->ball.col;

 orientation = ai_set_Vectang (pos_x,pos_y,ball_x,ball_y);

 if (state->ours[index].orientation == orientation)
 	{ if (pos_x *(state->field) < ball_x *(state->field))
			{mov->vel_ang[index] = - MAX_ANG_VEL;}
	  else {mov->vel_ang[index] = MAX_ANG_VEL;}
	}
 else
	{ai_set_angVel(state,mov,orientation);}
}

// Set the robot that must shoot by the parameter index
// Set if the shot goes to the nearest 1 or to the farest robot
void ai_get_shoot_rotate_force(stateGame *state, movements *mov, int index, int nearest)
{
 int pos_x;
 int pos_y;
 int valida; // indica si el llençament es factible
 int i; //indica index del propi robot
 int dir;

 pos_x = state->ours[index].row;
 pos_y = state->ours[index].col;

 //nearest or farest ??
 if (nearest)
 {
 	//The nearest robot ==> i = index +1
	//if the own[i+1] has obstacles i++;
	printf("Triada la opcio: nearest\n");
	valida = 0;
	i = 0;
	
	while ( !(valida) &&  (i < 5)) {
			valida = ai_obstacles_in_trajec(state,pos_x,pos_y, i, &dir);
			printf("direccio de llançament: %i, valida: %i\n",dir,valida);
			if (valida)
			{
				//Generar rotacio per xutar
				if (dir >= 0){	mov->vel_ang[index] = MAX_ANG_VEL; printf ("gir pel pase cap a la dreta direccio dreta\n");}
				else {mov->vel_ang[index] = - MAX_ANG_VEL;}
				break;
			}
			i++;		
	}
	if (!(valida)) // No es pot passar ==> llençar la pilota el més lluny possible
		{
				//Generar rotacio per xutar el més lluny possible		
				mov->vel_ang[index] = MAX_ANG_VEL;
				mov->vel_lin[index] = MAX_LIN_VEL;	
		}
		
	
 }
 else
 {
 	//The farest robot ==> i = 4
	//if the own[i] has obstacles i--;
	valida = 0;
	i = 4;
	while ( !(valida) &&  (i >= 0)) {
			valida = ai_obstacles_in_trajec(state, pos_x,pos_y, i, &dir);
			if (valida)
			{
				//Generar rotacio per xutar
				if (dir >= 0){	mov->vel_ang[index] = MAX_ANG_VEL;}
				else {mov->vel_ang[index] = - MAX_ANG_VEL;}
				break;
			}
			i--;
	}
	if (!(valida)) // No es pot passar ==> llençar la pilota el més lluny possible
		{
				mov->vel_ang[index] = MAX_ANG_VEL;
				mov->vel_lin[index] = MAX_LIN_VEL;
		}
 }

}
// Defines the lineal Velocity to a determinated robot that depends of a cartasian position
void ai_set_linVel_robot(stateGame *state, movements *mov, int index, int row, int col)
{
  int shoot_area;

	shoot_area = HALF_ROBOT + HALF_BALL;

	if (state->ours[index].row == (row -shoot_area)) {mov->vel_lin[index] = 0;}
	else 	
	{	
		if (state->ours[index].row > row)
		{
			//printf("Pilota sota fila\n");
			mov->vel_lin[index] = - MAX_LIN_VEL;
		}
		else	
		{
			//printf("Pilot sobre fila\n");
			mov->vel_lin[index] =  MAX_LIN_VEL;
		}		
	}
	
}
int ai_set_Vectang(int row1, int col1, int row2, int col2)
{
 int Drow;
 int Dcol;
 double angle;
	
	Drow = (row2 - row1);
	Dcol = (col2 - col1); 

	angle = (atan((double)(Dcol/Drow)) * 180 / PI);
	return((int)angle);
}

// Defines the Angular Velocity to a determinated robot that depends of a cartasian position
void ai_set_angVel_robot(stateGame *state, movements *mov, int index, int row, int col)
{
 int robot_row;
 int robot_col;
 double Drow;
 double Dcol;
 double angle;
 int shoot_area;

	shoot_area = HALF_ROBOT + HALF_BALL;
	
	

	robot_row = state->ours[index].row;
	robot_col = state->ours[index].col;
	Drow = (robot_row - row);
	Dcol = (robot_col - col); 

	angle = (int)(atan((double)(Dcol/Drow)) * 180 / PI);

	if ((robot_row == (row -shoot_area)) &&  (robot_col == (col-shoot_area)))
	{	
		 if (index == state->keeperPos) {mov->vel_ang[index]=270;} // down face
		
		 else
		 {
		   mov->vel_ang[index]= -(state->ours[index].orientation) + state->field * 180;
		 }	
		
	}

	else
	{
		if (robot_row < row )
		{
		 if (robot_col < col) 
			mov->vel_ang[index] = -(state->ours[index].orientation)+(int) angle; 
		 else mov->vel_ang[index] = - (state->ours[index].orientation) + (int) angle + 180;

		}
		else //robot_row > row
		{
	 	 if (robot_col < col) mov->vel_ang[index]= - (state->ours[index].orientation) +(int) angle + 180;
		 else mov->vel_ang[index] = - (state->ours[index].orientation)+ (int)angle;	
		}
	}

 	
}

// returns the angular velocity needed to get an determined orientation
int ai_set_angVel(stateGame *state, movements *mov, int orientation)
{
	//printf("S ==> Orientacio: %i maxima ANG_VEL %i\n",orientation, MAX_ANG_VEL);
	if (orientation == 0 ) {return (0);}
 	if (orientation > 0) 
		{
		//printf("S ==> Orientacio positiva\n");
		if (orientation >= MAX_ANG_VEL) return (MAX_ANG_VEL*state->field);
		else return (orientation*state->field);
		}
	else
		{
		//printf("Orientacio negativa\n");
		if (orientation <= -MAX_ANG_VEL) {return (-MAX_ANG_VEL*state->field);}
		else {return (-orientation*state->field);}
		}
}

void ai_set_Vel_robot_Simple(stateGame *state, movements *mov, int index, int row, int col)
{
 int robot_row;
 int robot_col;
 double Drow;
 double Dcol;
 double angle;
 int shoot_area;
 int new_orientation;

	shoot_area = HALF_ROBOT + HALF_BALL;
	

	robot_row = state->ours[index].row;
	robot_col = state->ours[index].col;
	Drow = (double)(robot_row - row);
	Dcol = (double)(robot_col - col); 

	angle = (int)(atan((double)(Drow/Dcol)) * 180 / PI);
	//printf(" robot %i dcol: %f drow: %f angle: %f\n",index,Dcol,Drow, angle);
	//printf("The robot %i (%i, %i) (%i, %i) shoot_area %i angle: %i\n",index, robot_row, robot_col,row,col,shoot_area,angle);
	if (( ((robot_row - row) *state->field) <= 0) &&  
		 (((robot_col - col)  *state->field) <= 0))
	{	
	  if (index == state->keeperPos) 
		{ new_orientation=state->ours[state->keeperPos].orientation;
		  new_orientation = 270 - new_orientation; // down face
		  if (abs(new_orientation <=10)){ mov->vel_ang[state->keeperPos]=0;}
		  else { if (new_orientation > 0) 
				{mov->vel_ang[state->keeperPos]=ai_set_angVel(state,mov,new_orientation);}
			 	 else {mov->vel_ang[state->keeperPos]=ai_set_angVel(state,mov,new_orientation);}
			}
				//   printf("Robot porter ben posicionat, mirar aball\n");
	 	} 
		
	  else
	 	{
		printf("The robot %i (Simple) the stop option\n",index);
		angle = 180;
		new_orientation = state->ours[index].orientation;
		new_orientation = angle  - new_orientation;		
	  	mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);	
	  	mov->vel_lin[index] = 0; //TOMAS ==> COMPROBAR BON FUNCIONAMENT O DONAR VELOCITAT  
	 	}	
		
	}

	else
	{
		if (robot_row < row )
		{
		 if (robot_col < col) 
			{

			new_orientation=state->ours[index].orientation;
			new_orientation = angle  - new_orientation;
			if (new_orientation ==0){ai_set_linVel_robot(state,mov,index,row,col);}
			else{mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);}
			
			/*mov->vel_lin[index] = MAX_LIN_VEL;*/
			//printf("%i ==> La bola esta a dalt a la dreta\n",index);
			//printf("The robot %i La bola esta dalt-dreta\n",index);
			}
		 else 
			{
			new_orientation=state->ours[index].orientation;
			//add 180 degrees 'se arctan trigonometric propertie
			new_orientation = (angle + 180)  - new_orientation;
			if (new_orientation ==0){ai_set_linVel_robot(state,mov,index,row,col);}
			else{mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);}
/*
			mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);
			mov->vel_lin[index] = MAX_LIN_VEL;
*/
			//printf("%i ==> La bola esta a dalt a l'esquerra\n",index);
			//printf("The robot %i La bola esta dalt-esquerra\n",index);
			}

		}
		else //robot_row > row
		{
	 	 if (robot_col < col) 
			{
			new_orientation=state->ours[index].orientation;
			//add 180 degrees 'se arctan trigonometric propertie
			new_orientation = (angle + 180)  - new_orientation;
			if (new_orientation ==0){ai_set_linVel_robot(state,mov,index,row,col);}
			else{mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);}
/*
			mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);
			mov->vel_lin[index] = MAX_LIN_VEL;
*/
			//printf("The robot %i La bola esta aball a la dreta\n",index);
			}
		 else 
			{
			//printf("The robot %i derive movement\n",index);
			new_orientation=state->ours[index].orientation;
			if (new_orientation ==0){ai_set_linVel_robot(state,mov,index,row,col);}
			else{mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);}
/*
			if (new_orientation == 180){ new_orientation = angle;}
			else {new_orientation = angle  - new_orientation;}
*/
			
			//printf("%i ==> Orientacio del robot:%i angle amb la bola:%i\n", index,new_orientation,angle);
/*						
			mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);	
			mov->vel_lin[index] = MAX_LIN_VEL;
*/
			//printf("%i ==> La bola esta aball a l'esquerra\n",index);
			}
		}
	}
}

// Defines the Angular Velocity to a determinated robot that depends of a cartasian position

//// TOMAS ==>  HAS DE MODIFICAR PER TENIR EN COMPTE LA EFICIENCIA DEL GIR
//		POTSER MES CURT FER UN GIR NEGATIU PER ARRIBAR AL MATEIX VECTOR DE DIRECCIO
//		EFICIENCIA DE LES VELOCITATS

void ai_set_Vel_robot(stateGame *state, movements *mov, int index, int row, int col)
{
 int robot_row;
 int robot_col;
 double Drow;
 double Dcol;
 double angle;
 int shoot_area;
 int new_orientation;

	shoot_area = HALF_ROBOT + HALF_BALL;
	

	robot_row = state->ours[index].row;
	robot_col = state->ours[index].col;
	Drow = (double)(robot_row - row);
	Dcol = (double)(robot_col - col); 

	angle = (atan((double)(Drow/Dcol)) * 180 / PI);
	//printf(" robot %i dcol: %f drow: %f angle: %f\n",index,Dcol,Drow, angle);

	if ((robot_row == (row -shoot_area)) &&  (robot_col == (col-shoot_area)))
	{	
	  if (index == state->keeperPos) 
		{ new_orientation=state->ours[state->keeperPos].orientation;
		  new_orientation = 270 - new_orientation; // down face
		  if (abs(new_orientation <=10)){ mov->vel_ang[state->keeperPos]=0;}
		  else { if (new_orientation > 0) 
				{mov->vel_ang[state->keeperPos]=ai_set_angVel(state,mov,new_orientation);}
			 else   {mov->vel_ang[state->keeperPos]=ai_set_angVel(state,mov,new_orientation);}
			}
				//   printf("Robot porter ben posicionat, mirar aball\n");
	 	} 
		
	  else
	 	{
		new_orientation=state->ours[index].orientation;
		new_orientation = angle  - new_orientation;
	  	mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);
	  	mov->vel_lin[index] = 0; //TOMAS ==> COMPROBAR BON FUNCIONAMENT O DONAR VELOCITAT  
	 	}	
		
	}

	else
	{
		if (robot_row < row )
		{
		 if (robot_col < col) 
			{

			new_orientation=state->ours[index].orientation;
			new_orientation = angle  - new_orientation;
			mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation); 
			mov->vel_lin[index] = MAX_LIN_VEL;
			//printf("%i ==> La bola esta a dalt a la dreta\n",index);
			}
		 else 
			{
			new_orientation=state->ours[index].orientation;
			//add 180 degrees 'se arctan trigonometric propertie
			new_orientation = (angle + 180)  - new_orientation;
			mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);
			mov->vel_lin[index] = MAX_LIN_VEL;
			//printf("%i ==> La bola esta a dalt a l'esquerra\n",index);
			}

		}
		else //robot_row > row
		{
	 	 if (robot_col < col) 
			{
			new_orientation=state->ours[index].orientation;
			//add 180 degrees 'se arctan trigonometric propertie
			new_orientation = (angle + 180)  - new_orientation;
			mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);
			mov->vel_lin[index] = MAX_LIN_VEL;
			//printf("%i ==> La bola esta aball a la dreta\n",index);
			}
		 else 
			{
			new_orientation=state->ours[index].orientation;
			
			if (new_orientation == 180){ new_orientation = angle;}
			else {new_orientation = angle  - new_orientation;}

			
			//printf("%i ==> Orientacio del robot:%i angle amb la bola:%i\n", index,new_orientation,angle);
						
			mov->vel_ang[index]= ai_set_angVel(state,mov,new_orientation);	
			mov->vel_lin[index] = MAX_LIN_VEL;
			//printf("%i ==> La bola esta aball a l'esquerra\n",index);
			}
		}
	}
}
//Moviment refinat porter
void ai_mov_refinat(stateGame *state, movements *mov, int index, int go_row, int go_col)
{

  int pos_interseccio;
  int orientation;
  int robot_angle;
  int mov_lang;
  int row;
  int col;
  int distance;

    row = state->ours[index].row;
    col = state->ours[index].col;
    distance = ai_euclidean_distance(row,col,go_row,go_col);
    printf("S ==> Moviment refintat\n");
    printf(" La pilota esta a (%i,%i)",go_row,go_col);
    orientation = ai_set_Vectang(row, col,go_row,go_col);
    robot_angle = state->ours[index].orientation;

    orientation = orientation - robot_angle;

    if (orientation == 0) { 
        mov->vel_ang[index] =0;
        if (distance >= MAX_LIN_VEL){ mov->vel_lin[index]=MAX_LIN_VEL;}
        else {mov->vel_lin[index]=distance;}
        }
    else {
        mov->vel_ang[index] = ai_set_angVel(state,mov,orientation);
    }
}

// Set the angular and lineal Velocity to move the robot between the ball and goal
void ai_pos_normal_ball_goal(stateGame *state, movements *mov, int index)
{

// greater than the top of the goal??
 if (index == state->keeperPos) // Goalkeeper
  {
	if (state->ball.row >= Region_Top_Goal)
		{
		ai_mov_refinat(state,mov,state->keeperPos,1000 ,(1100- 40)*state->field);
		//printf("La bola esta en la regio TOP_Goal del porter\n");
		}
	else
		{
		if (state->ball.row <= Region_Bottom_Goal)	
			{
			ai_mov_refinat(state,mov,state->keeperPos,800,(1100- 40)*state->field);
			printf("El robot es el goalkeeper ==>La bola esta en la regio BOTTOM_Goal del porter\n");
			}
			else
			{
			// the ball is in the DANGER area		
			//printf("La bola esta en la regio WARNING_Goal del porter\n");	
			ai_mov_refinat(state,mov,state->keeperPos,state->ball.row,(1100- 40)*state->field);
			}
		} 
  }
  else{ //Other robots
		ai_mov_refinat(state,mov,state->keeperPos,state->ball.row,(state->ball.col +70*state->field));
	  }
}

//Controla que el robot no es vagi fora del limits de la porteria
int ai_ctrl_wall(int row, int col, int *linVel, int *angVel)
{
 
  if ((Region_Top_Goal > row + (HALF_ROBOT)) || (Region_Bottom_Goal < row - (HALF_ROBOT)))
	{
		return (TRUE);
	}
  else
	{
		linVel=0;
		// TOMAS si fa falta fes moviment rotacio cap a la bola
		return(FALSE);
	}

}
// Indicates if the robot is too closer to shoot
int ai_get_shoot_position(stateGame *state, int index)
{
 int robot_row;
 int robot_col;
 float distance;

	robot_row = state->ours[index].row;
	robot_col = state->ours[index].col;

	distance = ai_euclidean_distance(state->ball.row, state->ball.col,robot_row,robot_col);

	if (distance < SHOOT_DISTANCE + HALF_BALL + HALF_ROBOT) {return (TRUE);}
	return(FALSE);
}

void ai_set_aproximation(stateGame *state,movements *mov,int index,int ball_row,int ball_col,int row,int col)
{
  int aux_row=900; //TOMAS PER DEFINIR EL CAMI EN CAS D'ATAC ==> PER FER
  int aux_col=1100* (state->field)*-1;

 	if (((col < ball_col) && (state->field == 1)) ||  //Aproximacio pel costat correcte
    ((col > ball_col) && (state->field == -1)))
			{
				//ens movem cap al mig de la porteria		
				//printf("The robot %i Aproximate to the enemi goal\n",index);
				ai_set_Vel_robot(state,mov,index,aux_row,aux_col); 
			}
	else
			{ 
				//printf("The robot %i Aproximation to the ball\n",index);
			ai_set_Vel_robot(state,mov,index,ball_row + BALL*state->field,ball_col+BALL+PROXIMITY_AREA); 
			}
}
/************************ Robot Movements ***************************/

int processState(stateGame *state, movements *mov)
{
 int i;
 int index;

	//// TOMAS AIXO S'HA DE TREURE ==> NOMES PER FER PROVES AMB INTERFICIES GRÀFIQUES
	//ai_set_MaxLin_Vel(40);
	//ai_set_MaxAng_Vel(30);
	

	//printf("S==> Mostrar estat obtingut del PYTHON\n");
	state->ownBall=set_teamOwn_ball(state);
	//printf("S==> El meu equip te la pilota?? %i\n",state->ownBall);
	set_initial_values(state);
    state->field =1;
	set_own_field(state);
	set_domination_robot_order(state);

	ai_print_state(state);
	
	//printf("S==> Posicio del porter %i %i\n",state->ours[0].row, state->ours[0].col);
 	for (i=0;i<5;i++)
	{
	 index = state->domRobotList[i];
	 //printf("Robot actual %i\n",index);
	
	mov->vel_lin[index] = 0;
	mov->vel_ang[index] = 0;
	printf("index %i, posicio del robot: %i\n",i,index);
	 switch (index)
		{
		case 0: {ai_goalkeeper_move(state,mov); }
			break;

		case 1: {//ai_robot_tonto(state,mov,index); 
			//ai_defender_NoAtac_move(state,mov,index);
			//printf("R%i==> angular: %i lineal: %i\n",index,mov->vel_ang[index],mov->vel_lin[index]);			
			}
			break;

		case 2: {//ai_defender_MayAtac_move(state,mov,2,1);
				//printf("R%i==> angular: %i lineal: %i\n",index,mov->vel_ang[index],mov->vel_lin[index]);
			 }
             break;

		case 3: {//ai_defender_MayAtac_move(state,mov,3,0);
			  printf("El robot 3 ja ha generat el seu moviment\n");
			 }break;

		case 4: {//printf("Moviment de robot %i encara no realitzat\n",state->domRobotList[i]);
			  //ai_defender_MayAtac_move(state,mov,3,0);
			 }break;
		}
		if (abs(mov->vel_lin[index])>MAX_LIN_VEL) { mov->vel_lin[index]=99;}
		if (abs(mov->vel_ang[index])>MAX_ANG_VEL) { mov->vel_ang[index]=99;}
	}
 
ai_print_movements(mov);
 return(TRUE);
 
}
void ai_robot_tonto(stateGame *state, movements *mov,int index)
{
  int pos[5][2];
  //int posi[][]={900,1000,900,1000,900,1000,900,1000,900,1000};
  int row;
  int col;
  int goal_row;
  int goal_col;
	
	pos[0][0] =  900; 
	pos[0][1] =	1000;
	pos[1][0] =  900;
	pos[1][1] = 1000;
	pos[2][0] =  900;
	pos[2][1] = 1000;
	pos[3][0] =  900;
	pos[3][1] = 1000;
	pos[4][0] =  900;
	pos[4][1] = 1000;

	goal_row = pos[index][0] *state->field*-1;
	goal_col = pos[index][1] *state->field*-1;
	row = state->ours[index].row;
	col = state->ours[index].col;

	if (state->ownBall == TRUE)
	{
		
		if (state->domRobotList[0] == index)// Robot no esta a prop
		{
			ai_set_aproximation(state,mov,index, goal_row, goal_col,row,col);			
		}
		else
		{
			ai_set_Vel_robot_Simple(state,mov,index,goal_row, goal_col);
		}
	}
	else
	{
			ai_set_Vel_robot_Simple(state,mov,index,goal_row, goal_col);
	}

}

// Goalkeeper
void ai_goalkeeper_move(stateGame *state, movements *mov)
{

	// Have we got the ball
	if (state->ownBall == TRUE)
	{
		//If the goalkeeper is the nearest robot then he has it.
        printf("El goalkeeper fara el moviment ");
		if (state->domRobotList[0] == state->keeperPos)
		{
			
			
			if (ai_get_shoot_position(state,state->keeperPos))
			{/*
			  printf("La pilota esta en posesió del porter\n");		

			  printf("Procedir a un pase direccional al company més llunya, ");
			  printf("tenir en compte  els contraris \n");			*/
			  //parameters: shooter, nearest= 1 farest= 0
			  ai_get_shoot_rotate_force(state,mov,state->keeperPos,1);
			}
			else 
			{
			  //printf("Pilota del porter, aproximacio a la pilota per pasar o xutar\n");

			  ai_set_Vel_robot(state,mov,state->keeperPos, state->ball.row, state->ball.col);
			  //printf("R%i==> El moviment del robot es:(%i,%i)\n",0, mov->vel_lin[0],mov->vel_ang[0]);
			}

		}
		else
		{
		// The ball is owned by one player of my team

		// Estrategia ==> posar-se entre la pilota i la porteria
		printf("R%i==>Em posare entre la pilota i la porteria\n",0);
		
		ai_pos_normal_ball_goal(state,mov,state->keeperPos); 
		}
		
	}
	else
	{
		//printf("La pilota esta en posesió del contrari posicionar-se amb normal a la porteria\n");
		ai_pos_normal_ball_goal(state,mov,state->keeperPos); 
	}

}

// Defensa que si pot marxa a atacar
void ai_defender_MayAtac_move(stateGame *state, movements *mov, int index, int up)
{
  int col;
  int row;
  int ball_row;
  int ball_col;
  int distance; //distancia entre la pilota i el robot
  int aux_row=900; //TOMAS PER DEFINIR EL CAMI EN CAS D'ATAC ==> PER FER
  int aux_col=1100* (state->field)*-1;
 
	col = state->ours[index].col;
	row = state->ours[index].row;
	ball_row = state->ball.row;
	ball_col = state->ball.col;
	
	

	printf("Defender MayAtac robot %i\n",index);

	if (state->ownBall == TRUE) 
	{
		printf("Tenim la pilota robot %i\n",index);
		if (state->domRobotList[0] == index)
		{

			//primer ens hem d'apropar a la pilota
			distance = ai_euclidean_distance(row,col,ball_row,ball_col);
			if (distance > (HALF_ROBOT + HALF_BALL))
				{ ai_set_Vel_robot_Simple(state,mov,index,ball_row,ball_col);return;}
	  			//printf("The Robot %i has the ball\n",index);
	  			// Movement to the center of the goal
	  			// if we are to closer to the oposite goal ==> SHOOT
	  			// 50 is the security distance to the Region B line to move centered
		
	  		if ((state->field * col)  >= (Region_Col_B - 50)  )
				{
			
				// Fer una funcio que dissenyi una funcio d'aproximacio amb la bola
				//printf("The Robot %i has SHOOT distance (%i)\n",index,col);
				ai_set_Vel_robot(state,mov,index, aux_row, aux_col); //Direccio centre de moment
				printf("Case 1 in the robot %i\n",index);
				}
	  		else  { //we are too closer to the oposite goal
				distance = ai_euclidean_distance(row,col,ball_row,ball_col);
			 	if (distance < (HALF_ROBOT + HALF_BALL)) 
					{
					//ens movem cap al mig de la porteria		
					//printf("The Robot %i goes to the enemi goal\n");
					ai_set_Vel_robot(state,mov,index,aux_row,aux_col); 
					printf("Case 2 in the robot %i\n",index);
					}
			 	else
					{
					ai_set_aproximation(state,mov,index, ball_row,ball_col,row,col);			
					printf("Case 3 in the robot %i\n",index);
			 		//printf("The Robot %i closer to the ball\n",index);
					/*
			 		if (state->ours[index].shooter == TRUE)
						{
						// Moure d'acord amb la extrategia triada
						ai_set_Vel_robot(state,mov,index, aux_row, aux_col);
						}
		 			else 
						{
						state->ours[index].shooter=TRUE;
						// Triar una estrategia
						state->ours[index].shoot_strategic = (rand() / RAND_MAX) * 4;
			
						// Fer moviment de la estrategia
						ai_set_Vel_robot(state,mov,index, aux_row, aux_col);
						}
					*/
					}

				}
			}
		else //no te la pilota ==> pero la te un company
		{
			printf("Case 4 in the robot %i\n",index);
			
			if (up){ai_set_Vel_robot_Simple(state,mov,index, HALF_FIELD_HEIGTH+100, Region_Col_B * state->field * -1);}
			else   {ai_set_Vel_robot_Simple(state,mov,index, HALF_FIELD_HEIGTH-100, Region_Col_B * state->field *-1 );}
			
			
			
		}
	}	
	else //No tenim la bola
		{
		printf("Case 5 in the robot %i\n",index);
	  	state->ours[index].shooter = FALSE;
	  	if (state->domRobotList[0] == index) //If the oposite robot is nearest to the ball
	  	{ai_set_Vel_robot_Simple(state,mov,index,ball_row,ball_col);}
		//ai_pos_normal_ball_goal(state,mov,index);return;}
		//printf("The Robot %i is closer to the ball that the other Robot\n",index);}
	  	else 
	  		{
			printf("Case 6 in the robot %i ==> vaig cap a la pilota\n",index);
			//This is the robot nearest to the ball of my team
			printf("Posicio de la pilota (%i, %i)\n",state->ball.row,state->ball.col);
			//ai_set_Vel_robot(state,mov,index, state->ball.row, state->ball.col);
			ai_set_Vel_robot_Simple(state,mov,index,ball_row,ball_col);
			//printf("The Robot %i is NOT closer to the ball that the other Robot\n",index);
			
	  	}
	}
}

void ai_defender_NoAtac_move(stateGame *state, movements *mov, int index)
{
 int ball_row;
 int ball_col;
 int row;
 int col;
 int distance;

 ball_row = state->ball.row;
 ball_col = state->ball.col;
 row = state->ours[index].row;
 col = state->ours[index].col;

 if (state->ownBall == TRUE)
	{
		if (state->domRobotList[0]==index)
			{
	  		//printf("The Robot %i has the ball\n",index);
	  		//printf("Action %i ==> Shoot",index);
			distance = ai_euclidean_distance(row,col,ball_row,ball_col);
			if ((distance > (HALF_ROBOT + HALF_BALL)) && (abs(state->ours[index].col) > Region_Col_B -400))	{ ai_set_Vel_robot_Simple(state,mov,index,ball_row,ball_col);return;}

	  		/*if (state->ours[index].col * (state->field) > ball_col * (state->field))
				{
				// Posicionament entre la pilota i el robot més proper
				//printf("Case 1 in robot %i\n",index);
				ai_set_Vel_robot(state,mov,index,ball_row + BALL*state->field,ball_col+BALL+PROXIMITY_AREA); 
				}*/
	  		else if (distance < (HALF_ROBOT + HALF_BALL))
					{
					//Shoot the ball
					//printf("The Robot %i decided to shoot the ball\n",index);
					//printf("Case 2 in robot %i\n",index);
					ai_set_shoot(state,mov,index);
					}
				else {ai_set_Vel_robot_Simple(state,mov,index,HALF_FIELD_HEIGTH,Region_Col_B *state->field);return;}
		}
		else	
		{
			//printf("The Robot %i not have the ball but the team have it\n",index);
			mov->vel_lin[index]=0;
			mov->vel_ang[index]=0;
			//printf("Case 3 in robot %i\n",index);
			ai_set_Vel_robot_Simple(state,mov,index,HALF_FIELD_HEIGTH,Region_Col_B * (state->field) );
		}
	}
 	else
	{
		//printf("The Robot %i decided to move to normal position with the ball\n",index);
		ai_pos_normal_ball_goal(state,mov,index);
	}
}
void ai_Killer_move(stateGame *state, movements *mov, int index)
{
  int col;
  int row;
  int ball_row;
  int ball_col;
  int aux_row=900; //TOMAS PER DEFINIR EL CAMI EN CAS D'ATAC ==> PER FER
  int aux_col=1100* (state->field)*-1;
 
	col = state->ours[index].col;
	row = state->ours[index].row;
	ball_row = state->ball.row;
	ball_col = state->ball.col;

	if (state->ownBall == TRUE) 
	{
		if (state->domRobotList[0] == index)
		{
	  	//printf("The Robot %i has the ball\n",index);
	  	// Movement to the center of the goal
	  	// if we are to closer to the oposite goal ==> SHOOT
	  	// 50 is the security distance to the Region B line to move centered
	  	if ((state->field * col)  >= (Region_Col_B - 50)  )
			{
			
			// Fer una funcio que dissenyi una funcio d'aproximacio amb la bola
			//printf("The Robot %i has SHOOT distance (%i)\n",index,col);
			ai_set_Vel_robot(state,mov,index, aux_row, aux_col); //Direccio centre de moment

			}
	  	else  { //we are too closer to the oposite goal
			 if ((col - state->ball.col) < PROXIMITY_AREA) 
				{
				//ens movem cap al mig de la porteria		
				//printf("The Robot %i goes to the enemi goal\n");
				ai_set_Vel_robot(state,mov,index,aux_row,aux_col); 
				}
			 else
				{
				ai_set_aproximation(state,mov,index, ball_row,ball_col,row,col);			
			 	//printf("The Robot %i closer to the ball\n",index);

				}

		}
		}
		else //no te la pilota ==> anem a cubrir la porteria
		{
			ai_set_Vel_robot(state,mov,index, ball_row, ball_col);
			
		}
	}	
	else
		{
	  	state->ours[index].shooter = FALSE;
	  	if (state->domRobotList[0] == index) //If the oposite robot is nearest to the ball
	  	{ai_pos_normal_ball_goal(state,mov,index);}
		//printf("The Robot %i is closer to the ball that the other Robot\n",index);}
	  	else //We are the team nearest to the ball 
	  		{
			//This is the robot nearest to the ball of my team
			ai_set_Vel_robot(state,mov,index, state->ball.row, state->ball.col);
			//printf("The Robot %i is NOT closer to the ball that the other Robot\n",index);
	  	}
	}
}
//---------------------------COPY STRUCTURES ----------------------------------------
void ai_copy_movements(movements *src, movements *dst)
{
 int i;
	for (i=0;i<5;i++)
		{
		dst->vel_lin[i]= src->vel_lin[i];
		dst->vel_ang[i]= src->vel_ang[i];
		}
}

void ai_copy_state(stateGame *src, stateGame *cpy)
{
  int i;

	for (i=0;i<5;i++)
		{
		cpy->ours[i].row = src->ours[i].row;
		cpy->ours[i].col = src->ours[i].col;
		cpy->theirs[i].row = src->theirs[i].row;
		cpy->theirs[i].col = src->theirs[i].col;
		}

	cpy->ball.row = src->ball.row;
	cpy->ball.col = src->ball.col;

	
	cpy->win = src->win;
	cpy->round = src->round;
}

//------------------------------ Prints ---------------------------------------------
void ai_print_state(stateGame *state)
{
  int i;
	printf("Goal: (%i, %i)\n", state->ball.row,state->ball.col);

	printf("Own robots:\n");
	for(i=0;i<5;i++)
		{printf("robot %i: (%i, %i) Orientation: %i\n",i,state->ours[i].row,state->ours[i].col,state->ours[i].orientation);}

	printf("Movement priority order: ");
	for(i=0;i<5;i++)
		{printf("%i ",state->domRobotList[i]);}
		printf("\n");
}

void ai_print_movements(movements *mov)
{
 int i;
 
 for (i=0;i<5;i++)
	{
	printf("Robot %i, vel_lin: %i vel_ang %i\n",i,mov->vel_lin[i],mov->vel_ang[i]);
	}
}

void ai_print_domRobots(stateGame *state)
{
int i;
 printf("Domintation robots in order: ");
 for (i=0;i<5;i++) printf("%i ", state->domRobotList[i]);
 printf("\n"); 
}
